﻿// for TDS01V (6-axis attitude device) 

using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Threading;
using System.Diagnostics;

[assembly: CLSCompliantAttribute(true)]
namespace Kakehashi.Device
{
    /// <summary>
    /// センサのステータスを返します
    /// </summary>
    public enum TDS01VState { DISCONNECTED, CONNECTED };

    /// <summary>
    /// 秋月 ３軸加速度・方位センサ TDS01V 処理クラス
    /// </summary>
    [CLSCompliantAttribute(true)]
    public class TDS01V : IDisposable
    {
        private TDS01VState status_ = TDS01VState.DISCONNECTED;
        private SerialPort serialPort_;
        private string portName_ = "";
        private float basePressure_ = 1013.0f;
        private float baseAzimuth_ = 0.0f;

        const string SERIAL_PORT_ERROR = "N/A";

        /// <summary>
        /// デバイスから取得した生データ。複数のスレッドで
        /// 読み書きするので注意。
        /// </summary>
        private string rawData_ = "";

        /// <summary>
        /// 別スレッドでのシリアルポート読み取りを制御するフラグ
        /// </summary>
        private bool isRunning_ = false;

        /// <summary>
        /// データ読み込みスレッド
        /// </summary>
        private Thread readThread_;

        /// <summary>
        /// デバイスの接続状況
        /// </summary>
        public TDS01VState Status { get { return status_; } }
        /// <summary>
        /// デバイス接続先のポート名
        /// </summary>
        public string PortName { get { return portName_; } }
        /// <summary>
        /// 海抜0mの気圧(hPa) (710.0hPa-1062.0hPaを設定)
        /// </summary>
        public float BasePressure
        {
            get { return basePressure_; }
            set { basePressure_ = value; }
        }
        /// <summary>
        /// 偏角 (0.0-360.0度) (真北を0とした東回りの角度)
        /// </summary>
        public float BaseAzimuth
        {
            get { return baseAzimuth_; }
            set { baseAzimuth_ = value; }
        }
        /// <summary>
        /// 秋月 ３軸加速度・方位センサ TDS01Vに接続します。
        /// </summary>
        /// <param name="portName">接続先ポート名 "COM2" など</param>
        public TDS01V(string portName)
        {
            ConnectTDS01V(portName);
        }

        /// <summary>
        /// 秋月 ３軸加速度・方位センサ TDS01Vに接続します。
        /// 接続先は自動的に検出します。
        /// </summary>
        public TDS01V()
        {
            string[] ports = System.IO.Ports.SerialPort.GetPortNames();
            for (int i = 0; i < ports.Length; i++)
            {
                ConnectTDS01V(ports[i]);
                if (status_ == TDS01VState.CONNECTED)
                {
                    return;
                }
            }
        }

        /// <summary>
        /// 気圧と方位角の校正を行います
        /// </summary>
        public void InitDevicePrams()
        {
            // パラメータ設定
            int bPressure = (int)(basePressure_ * 10.0f);
            int bAzimuth = (int)(baseAzimuth_ * 10.0f);
            string initParam = "0501" // 01 x 100ms = 100ms毎に計測
                + Convert.ToString(bPressure, 16).PadLeft(4, '0')
                + Convert.ToString(bAzimuth, 16).PadLeft(4, '0');
            ExecuteCommand(
                new string[] { initParam, "0DF7", "27" },
                new int[] { 2, 2, 2 });
            SendCommand("21"); // 計測開始
        }

        /// <summary>
        /// センサから値を取得します
        /// </summary>
        /// <returns>取得データ</returns>
        public TDS01VData GetData()
        {
            TDS01VData data = new TDS01VData();
            data.UTCTime = DateTime.UtcNow.ToFileTimeUtc();
            data.DeviceID = TDS01VData.DefalutDeviceID;

            if (status_ != TDS01VState.CONNECTED) return data;

            string str;
            lock (this)
            {
                str = rawData_; 
            }
            if (str != null && str.Length == 52)
            {
                data.MagX = Convert.ToInt16(str.Substring(0, 4), 16) / 10.0f;
                data.MagY = Convert.ToInt16(str.Substring(4, 4), 16) / 10.0f;
                data.MagZ = Convert.ToInt16(str.Substring(8, 4), 16) / 10.0f;
                data.Azimuth = Convert.ToInt16(str.Substring(12, 4), 16) / 10.0f;
                data.AccX = Convert.ToInt16(str.Substring(16, 4), 16) / 100.0f;
                data.AccY = Convert.ToInt16(str.Substring(20, 4), 16) / 100.0f;
                data.AccZ = Convert.ToInt16(str.Substring(24, 4), 16) / 100.0f;
                data.Roll = Convert.ToInt16(str.Substring(28, 4), 16) / 10.0f;
                data.Pitch = Convert.ToInt16(str.Substring(32, 4), 16) / 10.0f;
                data.Pressure = Convert.ToInt16(str.Substring(36, 4), 16) / 10.0f;
                data.Altitude = Convert.ToInt16(str.Substring(40, 4), 16) / 1.0f;
                data.Temperature = Convert.ToInt16(str.Substring(44, 4), 16) / 10.0f;
                data.Voltage = Convert.ToInt16(str.Substring(48, 4), 16) / 1.0f;
            }
            return data;
        }

        /// <summary>
        /// デバイスをクローズします。
        /// </summary>
        public void Close()
        {
            isRunning_ = false;

            if (readThread_ != null)
            {
                readThread_.Join();
                readThread_ = null;
            }

            if (serialPort_ != null)
            {
                serialPort_.Close();
                serialPort_ = null;
            }
            status_ = TDS01VState.DISCONNECTED;
            portName_ = "";
        }
        /// <summary>
        /// デバイスをDisposeします。内部でClose()を実行します。
        /// </summary>
        public void Dispose()
        {
            Close();
        }

        /// <summary>
        /// シリアルポートを初期化します
        /// </summary>
        /// <param name="portName">ポート名</param>
        private void InitSerialPort(string portName)
        {
            Close();

            portName_ = portName;

            serialPort_ = new SerialPort();
            serialPort_.PortName = portName_;
            serialPort_.BaudRate = 9600;
            serialPort_.DataBits = 8;
            serialPort_.Parity = System.IO.Ports.Parity.None;
            serialPort_.StopBits = System.IO.Ports.StopBits.One;
            serialPort_.ReadBufferSize  = 1024;
            serialPort_.WriteBufferSize = 1024;
            serialPort_.Handshake = System.IO.Ports.Handshake.None;
            serialPort_.Encoding = Encoding.ASCII;
            serialPort_.DtrEnable = true;
            serialPort_.RtsEnable = true;
            serialPort_.NewLine = "\r\n";
            serialPort_.ReadTimeout = 1500;
            serialPort_.WriteTimeout = 1500;

            try
            {
                serialPort_.Open();
                status_ = TDS01VState.CONNECTED;


            }
            catch
            {
                Close();
            }
        }

        /// <summary>
        /// TDS01Vへ接続します
        /// </summary>
        private void ConnectTDS01V(string portName)
        {
            // 初期化コマンド
            string[] sequence = {"2B", "0F", "2B", "2B",
                "33001E","333C1E","333C1E","335A1E","33781E","33961E","33B41E","33D21E",
                "33F00E","5F"};
            int[] expectedLength = {
                2,2,2,2,
                64,64,64,64,64,64,64,64,
                32, 4};

            InitSerialPort(portName);
            if (status_ == TDS01VState.CONNECTED)
            {
                ExecuteCommand(sequence, expectedLength);
                InitDevicePrams();

                isRunning_ = true;
                readThread_ = new Thread(Read);
                readThread_.IsBackground = true;
                readThread_.Start();
            }
        }

        /// <summary>
        /// センサにコマンドを発行して最後のデータを取得します
        /// </summary>
        /// <param name="sequence">コマンド群</param>
        /// <param name="expectedLength">各コマンド発行によって取得が期待されるデータの長さ</param>
        /// <returns>最後のコマンド発行によって得られた値</returns>
        private string ExecuteCommand(string[] sequence, int[] expectedLength)
        {
            string data = "";

            bool isReceived = true;
            for (int i = 0; i < sequence.Length; i++)
            {
                string res = SendCommand(sequence[i]);
                if (res == SERIAL_PORT_ERROR || res.Length != expectedLength[i])
                {
                    isReceived = false;
                    break;
                }

                // 一番最後に受信したデータを返す
                if (i == sequence.Length - 1)
                {
                    data = res;
                }
            }
            if (isReceived)
            {
                status_ = TDS01VState.CONNECTED;
            }
            else
            {
                Close();
            }
            return data;
        }
        /// <summary>
        /// TDS01VSに対してコマンド文字列を送信します
        /// </summary>
        /// <returns>TDS01VSからの応答文字列</returns>
        public string SendCommand(string command)
        {
            string req = WriteLine(command);
            if (req == SERIAL_PORT_ERROR)
            {
                return req;
            }
            string res = ReadLine();
            return res;
        }

        /// <summary>
        /// シリアルポートに1行分のデータを書き込みます
        /// </summary>
        /// <param name="command">送信文字列</param>
        /// <returns>正常時は送信文字列、異常時はエラー文字列</returns>
        private string WriteLine(string command)
        {
            try
            {
                serialPort_.WriteLine(command);
                return command;
            }
            catch (Exception)
            {
                Close();
                return SERIAL_PORT_ERROR;
            }
        }
        /// <summary>
        /// シリアルポートから1行分のデータを読み取ります
        /// </summary>
        /// <returns>正常時は受信文字列、異常時はエラー文字列</returns>
        private string ReadLine()
        {
            try
            {
                string res = serialPort_.ReadLine();
                return res;
            }
            catch (Exception)
            {
                Close();
                return SERIAL_PORT_ERROR;
            }
        }

        /// <summary>
        /// 別スレッドで実行されるデータ受信処理
        /// メインスレッドが _isRunning を false にするまでループ
        /// </summary>
        private void Read()
        {
            string str = "";

            while (isRunning_)
            {
                try
                {
                    serialPort_.WriteLine("29");
                    str = serialPort_.ReadLine();
                }
                catch (Exception)
                {
                    Close();
                    return;
                }
                if (str.Length == 52)
                {
                    lock (this)
                    {
                        rawData_ = str;
                    }
                }
                Thread.Sleep(200);
            }
        }

    }
}
